#include "wm_adc.h"
#include "math.h"

static int PolyfitInit(Polyfit_ParamTypeDef *PolyfitParam);
static void GaussSolve(int n, double a[], double x[], double b[]);
static void Polyfit(int n, double x[], double y[], uint32_t poly_n, double a[]);

static uint32_t _Get_Result(void)
{
	uint32_t value;
	
	value = ADC->DR;
	value = value & 0x3FFFF;
	if (value & 0x20000)
	{
		value &= 0x1FFFF;
	}
	else
	{
		value |= 0x20000;
	}
	return value;
}

HAL_StatusTypeDef HAL_ADC_Init(ADC_HandleTypeDef* hadc)
{
	uint32_t div;
	wm_sys_clk sysclk;
	
	if (hadc == NULL)
	{
		return HAL_ERROR;
	}
	
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	assert_param(IS_ADC_CHANNEL(hadc->Init.channel));
	assert_param(IS_ADC_FREQUENCY(hadc->Init.freq));
	
	hadc->Lock = HAL_UNLOCKED;
	
	HAL_ADC_MspInit(hadc);
	SET_BIT(hadc->Instance->ANA_CR, ADC_ANA_CR_LDOEN);
	PolyfitInit(&(hadc->PolyfitParam));
	
	SystemClock_Get(&sysclk);
	div = sysclk.apbclk * 1000000 / (hadc->Init.freq) / 512;
	MODIFY_REG(RCC->CLK_SEL, RCC_CLK_SEL_ADC_DIV, ((div & 0xFF) << RCC_CLK_SEL_ADC_DIV_Pos));
	SET_BIT(RCC->CLK_DIV, RCC_CLK_DIV_FREQ_EN);
	
	MODIFY_REG(hadc->Instance->ADC_CR, ADC_ADC_CR_DMAEN | ADC_ADC_CR_SWITCHTIME | ADC_ADC_CR_INITTIME | ADC_ADC_CR_ADCIE, 
	(0x50 << ADC_ADC_CR_SWITCHTIME_Pos) | (0x50 << ADC_ADC_CR_INITTIME_Pos));
	
	MODIFY_REG(hadc->Instance->PGA_CR, ADC_PGA_CR_BPREF | ADC_PGA_CR_GAIN | ADC_PGA_CR_BP, ADC_PGA_CR_CHOPEN | ADC_PGA_CR_PGAEN);
	
	// 校验计算offset
	MODIFY_REG(hadc->Instance->ANA_CR, ADC_ANA_CR_CH | ADC_ANA_CR_PD, ADC_ANA_CR_RST | ADC_ANA_CR_CH_OFFSET);
	
	HAL_ADC_PollForConversion(hadc);
	hadc->offset = _Get_Result();
	__HAL_ADC_DISABLE(hadc);
	
	return HAL_OK;
}

HAL_StatusTypeDef HAL_ADC_DeInit(ADC_HandleTypeDef* hadc)
{
	if (hadc == NULL)
	{
		return HAL_ERROR;
	}
	
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	
	__HAL_ADC_DISABLE(hadc);
	HAL_ADC_MspDeInit(hadc);
	__HAL_UNLOCK(hadc);
	
	return HAL_OK;
}

__attribute__((weak)) void HAL_ADC_MspInit(ADC_HandleTypeDef* hadc)
{
	UNUSED(hadc);
}

__attribute__((weak)) void HAL_ADC_MspDeInit(ADC_HandleTypeDef* hadc)
{
	UNUSED(hadc);
}

HAL_StatusTypeDef HAL_ADC_Start(ADC_HandleTypeDef* hadc)
{
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	
	__HAL_LOCK(hadc);
	MODIFY_REG(hadc->Instance->ANA_CR, ADC_ANA_CR_CH, hadc->Init.channel);
	__HAL_ADC_CLEAR_FLAG(hadc, ADC_IF_ADC | ADC_IF_CMP);
	__HAL_ADC_ENABLE(hadc);
	__HAL_UNLOCK(hadc);
	
	return HAL_OK;
}

HAL_StatusTypeDef HAL_ADC_Stop(ADC_HandleTypeDef* hadc)
{
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	
	__HAL_LOCK(hadc);
	__HAL_ADC_DISABLE(hadc);
	__HAL_UNLOCK(hadc);
	
	return HAL_OK;
}

HAL_StatusTypeDef HAL_ADC_PollForConversion(ADC_HandleTypeDef* hadc)
{
	uint32_t count = 0;
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instansce));
	
	while (1)
	{
		if (HAL_IS_BIT_SET(hadc->Instance->IF, (ADC_IF_ADC | ADC_IF_CMP)))
		{
			count++;
			__HAL_ADC_CLEAR_FLAG(hadc, (ADC_IF_ADC | ADC_IF_CMP));
			if(count == 4)
			{
				break;
			}
		}
	}
	
	return HAL_OK;
}

int HAL_ADC_GetValue(ADC_HandleTypeDef* hadc)
{
	int value = 0;
	
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	
	value = _Get_Result();
	
	return value;
}

int Hal_ADC_CalVoltage(ADC_HandleTypeDef *hadc, double vol)
{
	double y, voltage;
	int x = ((int)vol >> 2) & 0xFFFF;
	Polyfit_ParamTypeDef *param = &(hadc->PolyfitParam);
	
	if (hadc->Init.channel == ADC_CHANNEL_0_1 || hadc->Init.channel == ADC_CHANNEL_2_3)
	{
		voltage = ((double)vol - (double)(hadc->offset)) / 4.0;
		voltage = voltage * (126363 / 1000) / 1000000;
		
		y = voltage * 10000;
	}
	else
	{
		if (param->poly_n == 1)
		{
			y = param->a[1] * x + param->a[0];
		}
		else
		{
			voltage = ((double)vol - (double)(hadc->offset)) / 4.0;
			voltage = 1.196 + voltage * (126363 / 1000.0) / 1000000;
			
			y = voltage * 10000;
		}
	}
	
	return (int)(y / 10);
}

int HAL_ADC_GET_INPUT_VOLTAGE(ADC_HandleTypeDef* hadc)
{
	int value;
	
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	
	HAL_ADC_Start(hadc);
	HAL_ADC_PollForConversion(hadc);
	HAL_ADC_Stop(hadc);
	value = HAL_ADC_GetValue(hadc);
	
	return Hal_ADC_CalVoltage(hadc, (double)value);
}

HAL_StatusTypeDef HAL_ADC_Start_IT(ADC_HandleTypeDef* hadc)
{
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	
	__HAL_LOCK(hadc);
	MODIFY_REG(hadc->Instance->ANA_CR, ADC_ANA_CR_CH, hadc->Init.channel);
	__HAL_ADC_CLEAR_FLAG(hadc, ADC_IF_ADC | ADC_IF_CMP);
	__HAL_ADC_INT_ENABLE(hadc, ADC_ADC_CR_ADCIE);
	__HAL_ADC_ENABLE(hadc);
	__HAL_UNLOCK(hadc);
	
	return HAL_OK;
}

HAL_StatusTypeDef HAL_ADC_Stop_IT(ADC_HandleTypeDef* hadc)
{
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	
	__HAL_LOCK(hadc);
	__HAL_ADC_DISABLE(hadc);
	__HAL_ADC_INT_DISABLE(hadc, ADC_ADC_CR_ADCIE);
	__HAL_UNLOCK(hadc);
	
	return HAL_OK;
}

HAL_StatusTypeDef HAL_ADC_Start_Compare_IT(ADC_HandleTypeDef* hadc)
{
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	assert_param(IS_ADC_CMP_POL(hadc));
	
	__HAL_LOCK(hadc);
	__HAL_ADC_CLEAR_FLAG(hadc, ADC_IF_ADC | ADC_IF_CMP);
	MODIFY_REG(hadc->Instance->ANA_CR, ADC_ANA_CR_CH, hadc->Init.channel);
	MODIFY_REG(hadc->Instance->ADC_CR, ADC_ADC_CR_CMPPOL | ADC_ADC_CR_CMPEN, ((hadc->Init.cmp_pol) | ADC_ADC_CR_CMPEN));
	WRITE_REG(hadc->Instance->CMP_VAL, ((hadc->Init.cmp_val) & 0x3FFFF));
	__HAL_ADC_INT_ENABLE(hadc, ADC_ADC_CR_CMPIE);
	__HAL_ADC_ENABLE(hadc);
	__HAL_UNLOCK(hadc);
	
	return HAL_OK;
}

HAL_StatusTypeDef HAL_ADC_Stop_Compare_IT(ADC_HandleTypeDef* hadc)
{
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	
	__HAL_LOCK(hadc);
	__HAL_ADC_DISABLE(hadc);
	__HAL_ADC_INT_DISABLE(hadc, ADC_IF_CMP | ADC_ADC_CR_CMPEN);
	__HAL_UNLOCK(hadc);
	
	return HAL_OK;
}

__attribute__((weak)) void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
{
	UNUSED(hadc);
}

__attribute__((weak)) void HAL_ADC_CompareCallback(ADC_HandleTypeDef* hadc)
{
	UNUSED(hadc);
}

void HAL_ADC_IRQHandler(ADC_HandleTypeDef* hadc)
{
	assert_param(IS_ADC_ALL_INSTANCE(hadc->Instance));
	
	if (__HAL_ADC_GET_IT_SOURCE(hadc, ADC_ADC_CR_ADCIE))
	{
		if (__HAL_ADC_GET_FLAG(hadc, ADC_IF_ADC))
		{
			__HAL_ADC_CLEAR_FLAG(hadc, ADC_IF_ADC);
			HAL_ADC_ConvCpltCallback(hadc);
		}
	}
	if (__HAL_ADC_GET_IT_SOURCE(hadc, ADC_ADC_CR_CMPIE))
	{
		if (__HAL_ADC_GET_FLAG(hadc, ADC_IF_CMP))
		{
			__HAL_ADC_CLEAR_FLAG(hadc, ADC_IF_CMP);
			HAL_ADC_CompareCallback(hadc);
		}
	}
}

static int PolyfitInit(Polyfit_ParamTypeDef *PolyfitParam)
{
	Ft_AdcTypeDef adc_cal;
	float a = 0.0;
	float b = 0.0;
	int i;
	double x[8] = {0.0};
	double y[8] = {0.0};
	
	PolyfitParam->poly_n = 0;
	memset(&adc_cal, 0, sizeof(Ft_AdcTypeDef));
	HAL_Efuse_Get_ADC_Param(&adc_cal);
	
	if ((adc_cal.valid_cnt == 4) || (adc_cal.valid_cnt == 2) || (adc_cal.valid_cnt == 3))
	{
		a = adc_cal.a;
		b = adc_cal.b;
		if ((a > 1.0) && (a < 1.3) && (b < -1000.0)) // new calibration
		{
			PolyfitParam->poly_n = 1;
			PolyfitParam->a[1] = a;
			PolyfitParam->a[0] = b;
		}
		else // old calibration
		{
			for (i = 0; i < adc_cal.valid_cnt; i++)
			{
				x[i] = (double)adc_cal.units[i].real_val;
				y[i] = (double)adc_cal.units[i].ref_val;
			}
			PolyfitParam->poly_n = 1;
			Polyfit(adc_cal.valid_cnt, x, y, 1, PolyfitParam->a);
			if (b < -1000.0)
			{
				PolyfitParam->a[0] = b;
			}
		}
	}
	
	return 0;
}

static void GaussSolve(int n, double a[], double x[], double b[])
{
	int i, j, k, r;
	double max;
	
	for (k = 0; k < (n - 1); k++)
	{
		max = fabs(a[k * n + k]);
		r = k;
		for (i = (k + 1); i< (n - 1); i++)
		{
			if (max < fabs(a[i * n + i]))
			{
				max = fabs(a[i * n + i]);
				r = i;
			}
			
		}
		if (r != k)
		{
			for (i = 0; i < n; i++)
			{
				max = a[k * n + i];
				a[k * n + i] = a[r * n + i];
				a[r * n + i] = max;
			}
		}
		max = b[k];
		b[k] = b[r];
		b[r] = max;
		for (i = (k + 1); i < n; i++)
		{
			for (j = (k + 1); j < n; j++)
			{
				a[i * n + j] -= a[i * n + k] * a[k * n + j] / a[k * n + k];
			}
			b[i] -= a[i * n + k] * b[k] / a[k * n + k];
		}
	}
	
	for (i = (n - 1); i >= 0; x[i] /= a[i * n + i], i--)
	{
		for (j = (i + 1), x[i] = b[i]; j < n; j++)
		{
			x[i] -= a[i * n + j] * x[j];
		}
	}
}

static void Polyfit(int n, double x[], double y[], uint32_t poly_n, double a[])
{
	int i, j;
	double *tempx, *tempy, *sumxx, *sumxy, *ata;
	
	tempx = calloc(n, sizeof(double));
	tempy = calloc(n, sizeof(double));
	sumxx = calloc(poly_n * 2 + 1, sizeof(double));
	sumxy = calloc(poly_n + 1, sizeof(double));
	ata = calloc((poly_n + 1) * (poly_n + 1), sizeof(double));
	
	for (i = 0; i < n; i++)
	{
		tempx[i] = 1;
		tempy[i] = y[i];
	}
	for (i = 0; i < (2 * poly_n + 1); i++)
	{
		for (sumxx[i] = 0, j = 0; j < n; j++)
		{
			sumxx[i] += tempx[j];
			tempx[j] *= x[j];
		}
	}
	for (i = 0; i < (poly_n + 1); i++)
	{
		for (sumxy[i] = 0, j = 0; j < n; j++)
		{
			sumxy[i] += tempy[j];
			tempy[j] *= x[j];
		}
	}
	for (i = 0; i < (poly_n + 1); i++)
	{
		for(j = 0; j < (poly_n + 1); j++)
		{
			ata[i * (poly_n + 1) + j] = sumxx[i + j];
		}
	}
	
	GaussSolve(poly_n + 1, ata, a, sumxy);
	
	free(tempx);
	free(tempy);
	free(sumxx);
	free(sumxy);
	free(ata);
}

